#include <hidef.h> /* for EnableInterrupts macro */
#include "platform.h" /* include peripheral declarations */
#include "io.h"

#include "sci.h"
#include "string.h"
#include "term.h"
#include "i2c.h"
#include "disp.h"


volatile char result;


bool getIntState(void)
{
  #pragma MESSAGE DISABLE C1404        /* Suppress warning C1404: Return expected */
  asm 
  {
    CLRA
    BIL low
    COMA
    low:
  };
}


void hwTest(void)
{
  uint8 result, error = 0;
  termWrite("MC-CAR Hardware test...");
  
  result = i2cTest(0x1C << 1); // Accelerometer
  if (result != EC_SUCCESS)
  {
    termWriteLine("ERROR Accelerometer!");
    error++;
  }
 
    
  result = i2cTest(0x54); // Encoder
  if (result != EC_SUCCESS)
  {
    termWriteLine("ERROR Encoder!");
    error++;
  }
  
  result = i2cTest(0x39 << 1); // Color Sensor
  if (result != EC_SUCCESS)
  {
    termWriteLine("ERROR Color sensor!");
    error++;
  }
    
  result = i2cTest(0x78); // OLED Display
  if (result != EC_SUCCESS)
  {
    termWriteLine("ERROR OLED-Display!");
    error++;
  }
  
  if (error == 0) termWriteLine("No errors :-)");
}


void main(void) 
{
  EnableInterrupts;

  ioInit();
  sciInit(9600);
  i2cInit();
  sciWriteLine("mc bot ready :-)");
  ioSetLed(ledRear);
  
  hwTest();
  
  // configure accelerometer interrupt pin to open drain!
  result = i2cWriteCmdByte((0x1C << 1), 0x2C, 0x01); 
  
 
  // Stromversorgung OLED-Display einschalten
  result = i2cWriteCmdByte(0x54, 0x00, 0x20);
  
  // OLED Display Ansteuern
    result = displayOff();
    result = displayOn();
    result = initDisplay();
    result = setEntireDisplay();
    result = setContrast((unsigned char)0x00);   
//    result = setContrast((unsigned char)0xF0);
//    result = setContrast((unsigned char)0xFF);
    result = setAdressingMode(HORIZONTAL_MODE);    
    result = setColumns(0xAA);
    result = setDisplay();
    result = setContrast((unsigned char)0xFF); 
    //result = displayOff();
    
    
   // Stromversorgung OLED-Dispilay ausschalten
   //result = i2cWriteCmdByte(0x54, 0x00, 0x00);

 
  
  for(;;) 
  {
    __RESET_WATCHDOG();	/* feeds the dog */
  }
}
